#ifndef __OPTIMIZER_H__
#define __OPTIMIZER_H__
#include "../include/Map.hpp"
#include <math.h>
#include <g2o/core/sparse_block_matrix.h>
#include <g2o/types/sim3/types_seven_dof_expmap.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
#include "g2o/stuff/sampler.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include <g2o/types/slam3d/vertex_pointxyz.h>
#include "../include/Converter.hpp"
#include "../include/Frame.hpp"
// #include "../include/LoopClosing.hpp"
using namespace std;
namespace ORBSLAM
{
    // class Map;
    // class LoopClosing;
    class Optimizer
    {
    public:
        typedef pair<set<KeyFrame *>, int> type_consistent_group;
        typedef map<KeyFrame *, g2o::Sim3, less<KeyFrame *>, Eigen::aligned_allocator<pair<KeyFrame *const, g2o::Sim3>>> type_keyframe_and_pose;

    private:
        /* data */
    public:
        Optimizer(/* args */);
        ~Optimizer();
        void static Global_bundle_adjustment(Map *p_map, int iterations = 5, bool *pb_stopflag = NULL, const unsigned long n_loop_KF = 0, const bool b_robust = true);
        void static Bundle_adjustment(const vector<KeyFrame *> &vp_KF, const vector<MapPoint *> &vp_mpt, int iterations = 5, bool *pb_stopflag = NULL, const unsigned long n_loop_KF = 0, const bool b_robust = true);
        int static Pose_optimization(Frame *p_frame);

        void static Local_bundadjustment(KeyFrame *p_keyframe, bool *pb_stop_flag, Map *p_map);

        int static Optimize_sim3(KeyFrame *p_keyframe1, KeyFrame *p_keyframe2, vector<MapPoint *> &vp_matches1, g2o::Sim3 &g2oS12, const float th2, const bool b_fix_scale, Eigen::Matrix<double, 7, 7> &mat_accum_hessian, const bool b_all_points);

        void static Optimize_essential_graph(Map *p_map, KeyFrame *p_loop_keyframe, KeyFrame *p_cur_keyframe, const type_keyframe_and_pose &non_corrected_sim3, const type_keyframe_and_pose &corrected_sim3, const map<KeyFrame *, set<KeyFrame *>> &map_loop_connection, const bool &b_fix_scale);

    public:
        // LoopClosing *p_ds;
    };

}
#endif // __OPTIMIZER_H__